import math

import numpy as np


def get_directions(positions, velocities, targets, radius):
    num_robots = len(positions)
    safe_distance = 0.5  # 最小安全距离偏差
    max_speed = 6  # 最大速度
    new_positions = velocities    # 新位置
    new_velocities = positions   # 新速度
    # 计算每个机器人到其目标位置的方向向量
    directions = [targets[i] - positions[i] for i in range(num_robots)]

    # 对于每对机器人，计算它们之间的距离和最小安全距离
    for i in range(num_robots):
        for j in range(i + 1, num_robots):
            dist = np.linalg.norm(positions[i] - positions[j])
            if dist < radius[i] + radius[j] + safe_distance:
                print(dist)
                print('%d和%d会相撞' % (i, j))
                # 如果两个机器人之间的距离小于最小安全距离，则将它们的速度方向进行调整，使它们避免碰撞。
                avoid_direction = (positions[i] - positions[j]) / dist
                new_velocities[i] += avoid_direction
                new_velocities[j] -= avoid_direction
                print(velocities)
                print(new_velocities)
                print('速度更新')

    # 对于每个机器人，计算其可以移动的最大距离
    max_distances = [max_speed * np.linalg.norm(directions[i] / 50) for i in range(num_robots)]

    # 根据每个机器人的目标位置和最大速度确定它应该移动的距离
    movements = [directions[i] / np.linalg.norm(directions[i]) * min(max_speed, max_distances[i]) for i in
                 range(num_robots)]

    # 如果一个机器人将移动超过其可以移动的最大距离，则将其移动距离缩小为其可以移动的最大距离
    for i in range(num_robots):
        if np.linalg.norm(movements[i]) > max_distances[i]:
            movements[i] = movements[i] / np.linalg.norm(movements[i]) * max_distances[i]

    # 将每个机器人移动到下一个位置
    positions = [positions[i] + movements[i] for i in range(num_robots)]

    return new_positions, new_velocities

def calculate_orientation(vx, vy):
    """
    根据x轴速度和y轴速度计算朝向
    """
    orientation = math.atan2(vy, vx)
    return orientation

if __name__ == '__main__':
    # 当前机器人位置：[(7.363185883, 48.2604866), (16.385849, 48.25003815), (34.7057991, 48.28177643), (41.38605499, 48.25009155)]，
    # 朝向：[2.793721676, 2.79370594, 1.476671457, 2.793609619]，
    # 速度：[(-5.50103426, 2.395539999), (-4.739746094, 2.064110279), (0.2442743182, 2.587517977), (-4.739554882, 2.064558983)]，
    # 半径：[0.45, 0.45, 0.45, 0.45]，
    # 目标：[array([[ 4.75, 48.75]]), array([[14.75, 48.75]]), array([[34.75, 48.75]]), array([[39.75, 48.75]])]

    # 当前机器人位置：[(7.25037384, 48.30139542), (16.2918396, 48.2841301), (34.70993805, 48.32561493), (41.29204941, 48.28419113)]，
    # 朝向：[2.856556892, 2.84498024, 1.476673961, 2.845045328]，
    # 速度：[(-5.640604019, 2.045381546), (-4.700476646, 1.704558611), (0.2069244534, 2.191905737), (-4.700312614, 1.705011487)]，
    # 半径：[0.45, 0.45, 0.45, 0.45]，
    # 目标：[array([[ 4.75, 48.75]]), array([[14.75, 48.75]]), array([[34.75, 48.75]]), array([[39.75, 48.75]])]

    # 假设有四个机器人，它们的当前位置、运动方向和目标位置如下所示：
    positions = [np.array([1., 1.]), np.array([1., 2.]), np.array([6, 7]), np.array([41.38605499, 48.25009155])]
    velocities = [np.array([0., 6.]), np.array([0., -6.]), np.array([0.2442743182, 2.587517977]), np.array([-4.739554882, 2.064558983])]
    targets = [np.array([4.75, 48.75]), np.array([14.75, 48.75]), np.array([34.75, 48.75]), np.array([39.75, 48.75])]
    radius = [0.45, 0.45, 0.45, 0.45]
    # 获取每个机器人的下一步方向
    new_positions, new_velocities = get_directions(positions, velocities, targets, radius)

    print(velocities)
    print(new_velocities)
    # 输出每个机器人的新位置
    for i in range(len(new_positions)):
        # print(f"Robot {i}: {new_positions[i]}")
        # print(f"Robot {i}: {new_velocities[i]}")
        if "{:.5f}".format(velocities[i][0]) != "{:.5f}".format(new_velocities[i][0]) and "{:.5f}".format(velocities[i][1]) != "{:.5f}".format(new_velocities[i][1]):
            orientation = calculate_orientation(velocities[i][0], velocities[i][1])
            print(f"Robot {i}: {orientation}")
        else:
            print('不会碰撞')
